/*
 * @Author: EltonLi lijiaming@uavi-tech.com
 * @Date: 2023-09-12 18:09:57
 * @LastEditors: LVGRAPE
 * @LastEditTime: 2025-06-04 10:46:28
 * @FilePath: \zino-fc-v4\applications\ZINO_FC_V4.0\usercmd\usercmd.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */

#include "usercmd.h"
#include "fc_sensors.h"
#include "rc.h"
#include "maths.h"
#include "shanwai.h"
#include "zino.h"
#include "fc_error.h"
#include "pid.h"
#include "attitude.h"
#include "program.h"
#include "position.h"
#include <rtthread.h>
#include <stdint.h>
#include "statecal.h"

#define DBG_TAG "usercmd"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#define MOTOR_IDLING_PWM 200
#define MOTOR_WARMUP_RATE 10
#define RC_DEAD_BAND 10.f
#define RC_MID 512.f
#define THREAD_TIME_MS 10
#define ROLL_PITCH_ANGLE_LIMIT 30.f
#define YAW_ANGLERATE_LIMIT 50.0f
#define VEL_Z_LIMIT 60.0f

float groundHeight = 0;
#define FLY_HEIGHT_LIMIT 300
float takeoffHeight = 0.0f;
extern float F1_wave[12];
static float landingThrushold = 0;
static bool isReachHeightLimit = false;
float vx_kp_boost = 1.0f;
float vy_kp_boost = 1.0f;
float get_vx_kp_boost(void)
{
    return vx_kp_boost;
}
float get_vy_kp_boost(void)
{
    return vy_kp_boost;
}
float get_ground_height(void)
{
    return groundHeight;
}
float get_landingThrushold(void)
{
    return landingThrushold;
}
enum
{
    FLIPING_IDLE,
    FLIPING_GET_DIR,
    FLIPING_SET,
    FLIPING_SPEEDUP,
    FLIPING_SLOWDOWN,
    FLIPING_FLIPING,
    FLIPING_END,
    FLIPING_RECOVER,
    FLIPING_FORWARD,
    FLIPING_BACKWARD,
    FLIPING_LEFT,
    FLIPING_RIGHT,
    // FLIPING_FORWARD_LEFT,
    // FLIPING_FORWARD_RIGHT,
    // FLIPING_BACKWARD_LEFT,
    // FLIPING_BACKWARD_RIGHT,
};
// ZINO_COMMON_RC_CHANNLE_s zinoCommonRcData;

usercmd_t usercmd;
setPoint_t setPoint;

uint8_t testoptQual = 0;
static float thrustLpf = 0.f;
const uint16_t ReLockTime = 10000;
static uint16_t okToArmT;
static bool warmUp;
static int16_t warmUpPwm;
static bool landflag = false;
bool ReachTakingOffHeight = false;
// static uint32_t takeofftime = 0;
// static uint32_t landtime = 0;
static int16_t headingStart = 0;
// static int16_t maxRoll = 0, maxPitch = 0;
// static bool getMaxRoll = false;
// static bool getMaxPitch = false;

static int8_t flipingState = FLIPING_IDLE;
static int8_t flipingDir = FLIPING_IDLE;

static uint32_t movingStoptime_x = 0;
static uint32_t movingStoptime_y = 0;
static uint32_t movingStoptime_z = 0;
static uint8_t lowRcCnt = 0;

#define MIN_RC_INPUT 150
float getThrustLpf()
{
    return thrustLpf;
}

void CalibrationControl(usercmd_t *usercmd)
{
    // LOG_D("rlr:%d llr:%d %08X",usercmd->rcChannel[RC_RLR], usercmd->rcChannel[RC_LLR], fc_error_flag);
    // static bool calibrate = false;
    if (FC_ERROR_CHECK(FC_OK_TO_ARM) || FC_ERROR_CHECK(FC_ARMING_ALERK) || FC_ERROR_CHECK(FC_IMU_CALIBRATING) || FC_ERROR_CHECK(FC_ARMED))
        return;
    if (usercmd->rcDeflection[RC_RLR] > 0.5f && usercmd->rcDeflection[RC_LLR] < -0.5f) // 外一校准
    {
        static uint8_t caliCnt = 0;
        if (caliCnt++ > 50)
        {
            caliCnt = 0;
            FC_ERROR_SET(FC_IMU_CALIBRATING);
            AccStartCalibration();
            GyroStartCalibration();
        }
        // LOG_D("CalibrationControl %d %d",caliCnt,FC_ERROR_CHECK(FC_IMU_CALIBRATING));
    }
}
void Unlock(usercmd_t *usercmd)
{
    static uint16_t unLockT = 0;
    static uint16_t okToArmT = 0;
    static uint16_t warnFreq = 180;
    static uint8_t beepCnt;
    if (usercmd->rcDeflection[RC_RLR] < -0.5f && usercmd->rcDeflection[RC_RUD] < -0.5f && usercmd->rcDeflection[RC_LLR] > 0.5f && usercmd->rcDeflection[RC_LUD] < -0.5f) // 内八解锁
    {
        unLockT += THREAD_TIME_MS;
        if (unLockT > 1000)
        {
            FC_ERROR_SET(FC_ARMING_ALERK);

            unLockT = 0;
        }
    }
    else
    {
        unLockT = 0;
    }
    if (FC_ERROR_CHECK(FC_ARMING_ALERK))
    {

        // 电机发出警报声音
        static uint8_t beepT;
        static bool beep;
        beepT += THREAD_TIME_MS;
        motor_set_freq(1000); // 1.5kHz电机鸣叫
        if (beepT > warnFreq)
        {
            if (warnFreq > 100)
                warnFreq -= THREAD_TIME_MS;
            beepT = 0;
            beep = !beep;
            beepCnt++;
        }

        if (beepCnt > THREAD_TIME_MS)
        {
            FC_ERROR_RESET(FC_ARMING_ALERK);
            if (get_vbat_voltage_mv() < 6600) // NOTE 电量低时不能解锁
            {
                // armingAleakFlag = 0;
                FC_ERROR_SET(FC_LOW_POWER);
                FC_ERROR_RESET(FC_EMERGENCY_STOP);
                FC_ERROR_RESET(FC_OK_TO_ARM);
            }
            else
            {
                FC_ERROR_SET(FC_OK_TO_ARM);
            }

            // FC_ERROR_SET(FC_OK_TO_ARM);
            motor_set_freq(0); // 0时取默认值
        }

        usercmd->cmChannel[CMD_THROTTLE] = beep ? 5 : 0;
    }
    else
    {
        warnFreq = 180;
        beepCnt = 0;
    }
    if (FC_ERROR_CHECK(FC_OK_TO_ARM))
    {
        okToArmT += THREAD_TIME_MS;
        if (!warmUp)
        {

            static uint8_t stepTime;
            if (stepTime++ > 1)
            {
                stepTime = 0;
                if (warmUpPwm < MOTOR_IDLING_PWM)
                {
                    warmUpPwm += MOTOR_WARMUP_RATE;
                }
                else
                {
                    warmUpPwm = MOTOR_IDLING_PWM;
                    warmUp = true;
                }
            }

            usercmd->cmChannel[CMD_THROTTLE] = warmUpPwm;
        }
        else
        {

            if ((usercmd->rcDeflection[RC_LUD] > 0.25f) || (FC_ERROR_CHECK(FC_AUTO_TAKINGOFF)))
            {
                motor_set_freq(0); // 0时取默认值
                // 如果推油门就进入解锁状态
                FC_ERROR_SET(FC_ARMED);
                FC_ERROR_RESET(FC_OK_TO_ARM);
                // FC_ERROR_RESET(FC_EMERGENCY_STOP);
                okToArmT = 0;
                warmUp = false;
                warmUpPwm = 0;
                LOG_D("FC_ARMED\n");
                FC_ERROR_SET(FC_TAKINGOFF);
            }
            else
            {
                // NOTE 超时没推油门就上锁
                if ((okToArmT > ReLockTime) || (usercmd->rcChannel[RC_LUD] < -100))
                {
                    FC_ERROR_SET(FC_EMERGENCY_STOP);
                    FC_ERROR_RESET(FC_ARMED);
                    FC_ERROR_RESET(FC_OK_TO_ARM);
                }
            }
        }
    }
    else
    {
        warmUp = false;
        warmUpPwm = 0;
        okToArmT = 0;
    }
    if (FC_ERROR_CHECK(FC_EMERGENCY_STOP))
    {
        okToArmT = 0;
        warmUp = false;
        warmUpPwm = 0;
        usercmd->cmChannel[CMD_THROTTLE] = 0;
    }
}
int8_t fc_crash()
{
    float crashAngle = pidConfig()->maxTiltAngle > 45 ? pidConfig()->maxTiltAngle : 45;

    bool isFlitDown = getCosTiltAngle() < cos_approx(DEGREES_TO_RADIANS(crashAngle));
    // rt_printf_float("angleLimit: ", angleLimit, " ");
    // rt_printf_float("maxTiltAngle: ", pidConfig()->maxTiltAngle, " ");
    // rt_printf_float("getCosTiltAngle: ", getCosTiltAngle(), "\n");
    if (isFlitDown)
        rt_kprintf("flitDown: %d 0x%08X\n", isFlitDown, fc_error_flag);
    return ((isFlitDown) && (!FC_ERROR_CHECK(FC_FLIPING_MODE)));
}
void manul_mode_switch(usercmd_t *usercmd)
{
    static uint8_t intervalCnt = 0;
    if ((usercmd->rcChannel[RC_KEY_VALUE] == (1 << BALL_ADJ_LRIGHT)) && (intervalCnt == 0))
    {
        intervalCnt = 1000 / THREAD_TIME_MS;
        if (!FC_ERROR_CHECK(FC_MANUAL_MODE))
        {
            FC_ERROR_SET(FC_MANUAL_MODE);
            rt_kprintf("MANUAL_MODE\n");
        }
        else
        {
            FC_ERROR_RESET(FC_MANUAL_MODE);
            rt_kprintf("AUTO_MODE\n");
        }
    }
    if (intervalCnt > 0)
    {
        intervalCnt--;
    }
}
void headless_mode_switch(usercmd_t *usercmd)
{
    static uint8_t intervalCnt = 0;
    if ((usercmd->rcChannel[RC_KEY_VALUE] == (1 << BALL_ADJ_LDOWN)) && (intervalCnt == 0))
    {
        intervalCnt = 1000 / THREAD_TIME_MS;
        if (!FC_ERROR_CHECK(FC_HEADLESS_MODE))
        {
            FC_ERROR_SET(FC_HEADLESS_MODE);
            rt_kprintf("FC_HEADLESS_MODE\n");
        }
        else
        {
            FC_ERROR_RESET(FC_HEADLESS_MODE);
            headingStart = 0;
            rt_kprintf("FC_HEADLESS_MODE exit\n");
        }
    }
    if (intervalCnt > 0)
    {
        intervalCnt--;
    }
}
void fc_enbale_fliting(usercmd_t *usercmd, float dt)
{
    static float holdCnt = 0;
    if ((usercmd->rcChannel[RC_KEY_VALUE] & (1 << BALL_ADJ_LUP)) && !FC_ERROR_CHECK(FC_FLIPING_MODE))
    {
        holdCnt += dt;
        if (holdCnt > 1.5f)
        {
            holdCnt = 0;
            flipingState = FLIPING_GET_DIR;
            // FC_ERROR_SET(FC_FLIPING_MODE);
            rt_kprintf("FLIPING_MODE\n");
        }
    }
    else
    {
        // if (!FC_ERROR_CHECK(FC_FLIPING_MODE))
        holdCnt = 0;
    }
    // if (FC_ERROR_CHECK(FC_FLIPING_MODE))
    // {
    //     if (holdCnt++ > 10000 / THREAD_TIME_MS)
    //     {
    //         FC_ERROR_RESET(FC_FLIPING_MODE);
    //         rt_kprintf("FLIPING_MODE exit\n");
    //     }
    // }

    // rt_kprintf("holdCnt=%d\n", holdCnt);
}
void EmergencyStop(usercmd_t *usercmd)
{
    // rt_thread_mdelay(10);

    // rt_printf_float("cosTilt: ", getCosTiltAngle(), "\n");
    // NOTE - 急停
    static uint16_t stopT = 0;
    static uint16_t EmStopT = 0;
    if (FC_ERROR_CHECK(FC_ARMED) || FC_ERROR_CHECK(FC_OK_TO_ARM) || FC_ERROR_CHECK(FC_ARMING_ALERK))
    {
        const uint32_t accInpactLimit = 10000;
        bool isAccCrash = ((ABS(zroneEstimator.acc[0]) > accInpactLimit) || (ABS(zroneEstimator.acc[1]) > accInpactLimit) || (ABS(zroneEstimator.acc[2]) > accInpactLimit));
        if (isAccCrash)
        {
            FC_ERROR_RESET(FC_ARMED);
            FC_ERROR_RESET(FC_OK_TO_ARM);
            FC_ERROR_RESET(FC_ARMING_ALERK);
            FC_ERROR_SET(FC_EMERGENCY_STOP);
            LOG_E("Acc_CRASH\n");
        }
        if ((usercmd->rcChannel[RC_KEY_VALUE] == (1 << BALL_ADJ_LLEFT)) || fc_crash())
        {
            EmStopT += THREAD_TIME_MS;
            if (EmStopT > 300)
            {

                FC_ERROR_RESET(FC_ARMED);
                FC_ERROR_RESET(FC_OK_TO_ARM);
                FC_ERROR_RESET(FC_ARMING_ALERK);
                FC_ERROR_SET(FC_EMERGENCY_STOP);
                LOG_D("EMERGENCY_STOP\n");

                EmStopT = 0;
            }
        }
        else
        {
            EmStopT = 0;
        }
    }

    if (FC_ERROR_CHECK(FC_EMERGENCY_STOP))
    {
        FC_ERROR_RESET(FC_ARMED);
        FC_ERROR_RESET(FC_OK_TO_ARM);
        FC_ERROR_RESET(FC_FLIPING_MODE); // FLIPING OVER
        FC_ERROR_RESET(FC_TAKINGOFF);
        FC_ERROR_RESET(FC_AIRBORN);
        FC_ERROR_RESET(FC_LANDING);
        FC_ERROR_RESET(FC_LANDED);
        okToArmT = 0;
        warmUp = false;
        warmUpPwm = 0;
        usercmd->cmChannel[CMD_THROTTLE] = 0;
        stopT += THREAD_TIME_MS;
        if (stopT > 1000)
        {
            LOG_E("EMERGENCY_STOP\n");
            FC_ERROR_RESET(FC_EMERGENCY_STOP);
            pro_event_send(PRO_EVENT_STOP);
            stopT = 0;
        }
    }
}

void AutoLandingHandle(usercmd_t *usercmd)
{

    if (fc_sensor_data.tof.range < 5000 && fc_sensor_data.tof.quality > 10 && fc_sensor_data.tof.range > 1)
    {
        if (fc_sensor_data.tof.range >= 50)
        {
            usercmd->rcChannel[RC_LUD] = -412;
        }
        else
        {
            FC_ERROR_RESET(FC_ARMED);
            FC_ERROR_RESET(FC_OK_TO_ARM);
            FC_ERROR_RESET(FC_ARMING_ALERK);

            landflag = false;
            ReachTakingOffHeight = false;
            pro_autotakingoff = false;
            pro_autolanding = false;

            pro_event_send(PRO_EVENT_LANDED);
            pro_event_state_set(PRO_EVENT_LANDED);
        }
    }
    else
    {
        if (zdrone_state.pos.z > 50)
        {
            usercmd->rcChannel[RC_LUD] = -412;
        }
        else
        {
            FC_ERROR_RESET(FC_ARMED);
            FC_ERROR_RESET(FC_OK_TO_ARM);
            FC_ERROR_RESET(FC_ARMING_ALERK);

            landflag = false;
            ReachTakingOffHeight = false;
            pro_autotakingoff = false;
            pro_autolanding = false;

            pro_event_send(PRO_EVENT_LANDED);
            pro_event_state_set(PRO_EVENT_LANDED);
        }
    }
}
void get_rt_lt_triger(usercmd_t *usercmd)
{
    // 检查程序是否准备好
    // 检查程序是否准备好
    if (FC_ERROR_CHECK(FC_PRORAM_READY))
    // 定义静态变量，用于记录LT和RT按键按下的时间
    {
        static uint16_t rtTime = 0;
        // 检查LT按键是否按下
        static uint16_t ltTime = 0;
        if (usercmd->rcChannel[RC_KEY_VALUE] & (1 << BALL_KEY_LT))
        // 如果LT按键按下的时间小于100ms，则时间加1
        {
            if (ltTime < (100 / THREAD_TIME_MS))
                // 如果LT按键按下的时间等于100ms，则发送LT触发事件
                ltTime++;
            if (ltTime == (100 / THREAD_TIME_MS - 1))
            {
                rt_kprintf("ltTime: %d\n", ltTime);
                pro_event_send(PRO_EVNET_LT_TRIGGER);
            }
        }
        else
        // 如果LT按键没有按下，则时间清零
        {
            ltTime = 0;
            // 检查RT按键是否按下
        }
        if (usercmd->rcChannel[RC_KEY_VALUE] & (1 << BALL_KEY_RT))
        // 如果RT按键按下的时间小于100ms，则时间加1
        {
            if (rtTime < (100 / THREAD_TIME_MS))
                // 如果RT按键按下的时间等于100ms，则发送RT触发事件
                rtTime++;
            if (rtTime == (100 / THREAD_TIME_MS - 1))
            {
                rt_kprintf("rtTime: %d\n", rtTime);
                pro_event_send(PRO_EVNET_RT_TRIGGER);
            }
        }
        else
        // 如果RT按键没有按下，则时间清零
        {
            rtTime = 0;
        }
    }
}
// 函数：fc_takingoff_handle
// 功能：处理飞机起飞
// 参数：usercmd_t *usercmd
void fc_takingoff_handle(usercmd_t *usercmd)
{
    static bool getGroundHeight = false;
    if (FC_ERROR_CHECK(FC_ARMED))
    {
        if (!getGroundHeight)
        {
            getGroundHeight = true;
            groundHeight = zdrone_state.pos.z;
            LOG_D("groundHeight=%d\n", (int)groundHeight);
            // rt_kprintf("groundHeight=%d\n", groundHeight);
        }
    }
    else
    {
        getGroundHeight = false;
        groundHeight = 0;
    }
    // 定义一个静态变量，用于记录起飞时间
    static uint32_t takingoffT = 0;
    takeoffHeight = groundHeight + 80.0f;
    // 检查飞机是否处于起飞状态
    if (FC_ERROR_CHECK(FC_TAKINGOFF))
    {
        // 如果起飞时间未记录，则记录当前时间
        if (takingoffT == 0)
        {
            takingoffT = millis();
            setPoint.Vel.z = 0;
            LOG_D("takingoffT=%d\n", takingoffT);
            // rt_kprintf("takingoffT=%d\n", takingoffT);
            // rt_kprintf("takingoffT=%d\n", takingoffT);
        }
        // 检查飞机是否已经起飞
        if (!FC_ERROR_CHECK(FC_AIRBORN))
        {
            // 检查飞机是否处于自动起飞状态
            if (FC_ERROR_CHECK(FC_AUTO_TAKINGOFF))
            {
                // 设置飞机的高度为80.0f
                setPoint.Pos.z = takeoffHeight;
                setPoint.manual[Z] = true;
                if (setPoint.Vel.z < 80)
                {
                    setPoint.Vel.z += 1.f;
                }
                // rt_printf_float("setPoint.Pos.z ", takeoffHeight, " \n");
            }
            if (zdrone_state.pos.z >= takeoffHeight)
            {
                // setPoint.manual [Z] = 0;
                // setPoint.Vel.z = 0;
                // takingoffT = 0;

                LOG_D("AIRBORNE\n");
                FC_ERROR_SET(FC_AIRBORN);
                // FC_ERROR_RESET(FC_TAKINGOFF);
                // FC_ERROR_RESET(FC_AUTO_TAKINGOFF);
                // pro_event_send(PRO_EVENT_AIRBORNE);
            }
            if ((millis() > (takingoffT + 5000)) && (takingoffT != 0))
            {
                setPoint.Pos.z = zdrone_state.pos.z;

                FC_ERROR_SET(FC_AIRBORN);
                LOG_D("timeout, AIRBORNE. Height:%d\n", (int)zdrone_state.pos.z);
            }
            if (FC_ERROR_CHECK(FC_AIRBORN))
            {
                setPoint.manual[Z] = 0;
                setPoint.Vel.z = 0;
                takingoffT = 0;
                FC_ERROR_RESET(FC_TAKINGOFF);
                FC_ERROR_RESET(FC_AUTO_TAKINGOFF);

                LOG_D("takeofoff done\n");

                pro_event_send(PRO_EVENT_AIRBORNE);
            }
            // rt_kprintf("takeoff height=%d\n", (int)zdrone_state.pos.z);
        }
    }
    else
    {
        takingoffT = 0;
    }
}
void fc_landing_handle(usercmd_t *usercmd)
{
    static uint32_t landingT = 0;
    static uint32_t manualLangdingT = 0;
    // rt_printf_float("thrust: ",fc_control.thrust, " \n");
    if (usercmd->rcDeflection[RC_LUD] <= -0.49f)
    {
        if (manualLangdingT == 0)
        {
            manualLangdingT = millis();
        }
    }
    else
    {
        manualLangdingT = 0;
    }
    if ((manualLangdingT != 0) && (millis() > (manualLangdingT + 1000)))
    {
        FC_ERROR_SET(FC_LANDING);
    }

    if (FC_ERROR_CHECK(FC_LANDING))
    {
        if (usercmd->rcDeflection[RC_LUD] > 0.45f)
        {
            FC_ERROR_RESET(FC_LANDING);
            manualLangdingT = 0;
        }
        // rt_kprintf("landing:%d\n",(int)setPoint.Pos.z);
        if (landingT == 0)
        {
            landingT = millis();
            // landingThrushold = fc_control.thrust * 0.85f;
            // rt_printf_float("landingHeight: ", setPoint.Pos.z, " \n");
            // rt_printf_float("fc_control.thrust: ", fc_control.thrust, " \n");

            // rt_kprintf("landingT=%d landingThrushold=%d, fc_control.thrust:%d\n", landingT, landingThrushold,(int)fc_control.thrust);
        }
        if (!FC_ERROR_CHECK(FC_LANDED))
        {
            // rt_kprintf("landing:%d\t",setPoint.manual[Z]);
            //  rt_printf_float("landingHeight: ", setPoint.Pos.z, " \n");

            setPoint.Pos.z -= 0.5f;

            bool isAccTouchDown = (ABS(zroneEstimator.acc[2]) > 500);
            bool isLaserTouchdown = (fc_sensor_data.tof.quality > 0) && (fc_sensor_data.tof.range > 10) && (fc_sensor_data.tof.range < 50);
            if (FC_ERROR_CHECK(FC_MANUAL_MODE))
            {
                if ((ABS(zroneEstimator.acc[2]) > 500) || isLaserTouchdown)
                {
                    FC_ERROR_RESET(FC_EMERGENCY_STOP);
                    landingThrushold = 0;
                    landingT = 0;
                    manualLangdingT = 0;
                    LOG_D("MANUL_LANDED\n");
                }
            }
            if (isLaserTouchdown)
            {
                rt_kprintf("isLaserTouchdown\n");
            }
            if (isAccTouchDown)
            {
                rt_kprintf("isAccTouchDown\n");
            }
            if (isAccTouchDown || isLaserTouchdown)
            {
                LOG_D("LANDED\n");
                FC_ERROR_RESET(FC_LANDING);
                FC_ERROR_RESET(FC_ARMED);
                FC_ERROR_RESET(FC_AIRBORN);
                FC_ERROR_SET(FC_LANDED);
                landingThrushold = 0;
                landingT = 0;
                manualLangdingT = 0;
                pro_event_send(PRO_EVENT_LANDED);
            }
        }
    }
    else
    {
        landingT = 0;
        landingThrushold = 0;
        FC_ERROR_RESET(FC_LANDED);
    }
}
void AutoTakingOffHandle(setPoint_t *set, zdrone_state_t *state)
{
    if (!ReachTakingOffHeight)
    {
        set->Pos.z = 80.0f;
        if (state->pos.z >= 80.0f)
        {
            ReachTakingOffHeight = true;
            pro_autotakingoff = false;

            pro_event_send(PRO_EVENT_AIRBORNE);
            pro_event_state_set(PRO_EVENT_AIRBORNE);
        }
        LOG_D("state->pos.z=%d\n", (int)state->pos.z);
    }
}

void switch_program_mode(usercmd_t *usercmd)
{
    static uint16_t unLockT = 0;
    static uint32_t longPressT = 0;
    if ((usercmd->rcChannel[RC_KEY_VALUE] & (1 << BALL_KEY_RT)) && (usercmd->rcChannel[RC_KEY_VALUE] & (1 << BALL_KEY_LT)))
    {
        if (unLockT < (2000 / THREAD_TIME_MS))
        {
            unLockT++;
        }
        if (unLockT == (1500 / THREAD_TIME_MS))
        {
            longPressT = millis();
            if (FC_ERROR_CHECK(FC_PROGRAM_MODE))
            {
                rt_kprintf("PROGRAM_MODE exit\n");
                FC_ERROR_RESET(FC_PROGRAM_MODE);
                FC_ERROR_RESET(FC_PRORAM_READY);
                pro_event_send(PRO_EVENT_DISABLE);
            }
            else
            {
                FC_ERROR_SET(FC_PROGRAM_MODE);
            }
        }
    }
    else
    {
        unLockT = 0;
        if ((longPressT != 0) && millis() > (longPressT + 1000))
        {
            longPressT = 0;
            if (FC_ERROR_CHECK(FC_PROGRAM_MODE))
            {
                rt_kprintf("FC_PRORAM_READY\n");
                FC_ERROR_SET(FC_PRORAM_READY);
                // if (check_pro_event_enable())
                // {
                //     rt_kprintf("PROGRAM_MODE\n");
                //     pro_event_start();
                // }
            }
        }
    }
}

void rc_data_fuc_handle(usercmd_t *usercmd)
{
    struct rc_handle *rccmd = get_rc_channel_ram();
    usercmd->maxYawRate = pidConfig()->rcRollPitchMax * 3;
    usercmd->maxRollAndPicthAngle = pidConfig()->rcRollPitchMax;
    usercmd->maxVel_Z = pidConfig()->maxRasing;

    // NOTE - 信号量小于3说明通讯中断
    usercmd->rc_rssi = rccmd->rx_rssi;
    static uint16_t rc_lost_time = 0;
    // static bool rc_lost_flag = false;

    if (usercmd->rc_rssi < 3)
    {
        FC_ERROR_SET(FC_RC_LOST);
        rc_lost_time += THREAD_TIME_MS;
        if (rc_lost_time > 3000)
        {
            // if (FC_ERROR_CHECK(FC_ARMED))
            //     rc_lost_flag = true;
        }
        else
        {
            for (uint8_t i = RC_RLR; i < RC_COUNT; i++)
            {
                usercmd->rcChannel[i] = 0;
            }
            // LOG_D("clean usercmd\n");
        }
    }
    else
    {
        rc_lost_time = 0;
        // rc_lost_flag = false;
        FC_ERROR_RESET(FC_RC_LOST);
        for (uint8_t i = RC_RLR; i < RC_COUNT; i++)
        {
            if (i <= RC_LUD)
            {
                usercmd->rcChannel[i] = applyDeadband(rccmd->rx_msg.v.channel[i] - RC_MID, RC_DEAD_BAND);
                usercmd->rcDeflection[i] = constrainf((float)usercmd->rcChannel[i] / (RC_MID - RC_DEAD_BAND), -1.0f, 1.0f);
                // usercmd->rcChannel[i] = rccmd->rx_msg.v.channel[i];
            }
            else
            {
                usercmd->rcChannel[i] = rccmd->rx_msg.v.channel[i];
            }
        }
    }
    // rt_printf_float("RC_RLR: ",usercmd->rcDeflection[RC_RLR], " ");
    // rt_printf_float("RC_RUD: ",usercmd->rcDeflection[RC_RUD], " ");
    // rt_printf_float("RC_LLR: ",usercmd->rcDeflection[RC_LLR], " ");
    // rt_printf_float("RC_LUD: ",usercmd->rcDeflection[RC_LUD], " \n");
    // NOTE - 解锁——校准——怠速——起飞

    CalibrationControl(usercmd);
    Unlock(usercmd);
    EmergencyStop(usercmd);
    manul_mode_switch(usercmd);
    switch_program_mode(usercmd);

    fc_takingoff_handle(usercmd);

    fc_landing_handle(usercmd);

    get_rt_lt_triger(usercmd);
    // rt_kprintf("usercmd->rcChannel[RC_KEY_VALUE]:%d, %d - %d\n", usercmd->rcChannel[RC_KEY_VALUE], ltTime, rtTime);
    // if (pro_autolanding || rc_lost_flag)
    // {
    //     if ((takeofftime != 0) && (millis() > (takeofftime + 2000)) && (landflag == false) && (ReachTakingOffHeight))
    //     {
    //         landflag = true;
    //         takeofftime = 0;
    //         landtime = millis();
    //         // LOG_D("landflag\n");
    //     }
    // }
    // if (landflag)
    // {
    //     AutoLandingHandle(usercmd);
    // }
    // rt_kprintf("usercmd->rcChannel[RC_KEY_VALUE]:%d\n", usercmd->rcChannel[RC_KEY_VALUE]);
}

void headless_mode_handle(setPoint_t *set, usercmd_t *usercmd, zdrone_state_t *state)
{
    // static bool moving = 0;
    if (FC_ERROR_CHECK(FC_HEADLESS_MODE))
    {
        if (headingStart == 0)
        {
            headingStart = state->attitude.yaw * 100.f;
            // rt_kprintf("headingStart: %d\n", headingStart);
        }
        const float radDiff = degreesToRadians(state->attitude.yaw - headingStart * 0.01f);
        const float cosDiff = cos_approx(radDiff);
        const float sinDiff = sin_approx(radDiff);

        float roll_angle = usercmd->rcDeflection[RC_RLR];
        float pitch_angle = usercmd->rcDeflection[RC_RUD];

        float rollExpect = cosDiff * roll_angle + sinDiff * pitch_angle;
        float pitchExpect = cosDiff * pitch_angle - sinDiff * roll_angle;

        usercmd->headFreeCh[RC_RUD] = pitchExpect;
        usercmd->headFreeCh[RC_RLR] = rollExpect;
    }
    else
    {
        headingStart = 0;
    }
}

uint8_t getFlipingDir(usercmd_t *usercmd)
{
    static uint8_t rcRollCnt = 0;
    static uint8_t rcPitchCnt = 0;

    float rcRoll = (float)(usercmd->rcChannel[RC_RLR] / (RC_MID - RC_DEAD_BAND));
    float rcPitch = (float)(usercmd->rcChannel[RC_RUD] / (RC_MID - RC_DEAD_BAND));

    flipingDir = FLIPING_IDLE;
    // if (flipingState == FLIPING_GET_DIR)
    // {
    if (ABS(rcRoll) > 0.3f)
    {
        if (rcRollCnt <= 10)
            rcRollCnt++;
    }
    else
    {
        rcRollCnt = 0;
    }
    if (ABS(rcPitch) > 0.3f)
    {
        if (rcPitchCnt <= 10)
            rcPitchCnt++;
    }
    else
    {
        rcPitchCnt = 0;
    }

    if (rcPitchCnt >= 10)
    {
        if (rcPitch > 0.3f)
        {
            flipingDir = FLIPING_FORWARD;
        }
        else if (rcPitch < -0.3f)
        {
            flipingDir = FLIPING_BACKWARD;
        }
    }
    if (rcRollCnt >= 10)
    {
        if (rcRoll > 0.3f)
        {
            flipingDir = FLIPING_RIGHT;
        }
        else if (rcRoll < -0.3f)
        {
            flipingDir = FLIPING_LEFT;
        }
    }
    if (flipingDir != FLIPING_IDLE)
    {
        flipingState = FLIPING_SET;
        return flipingDir;
    }
    // if (rcPitchCnt > 8 && rcRollCnt > 8)
    // {
    //     if (rcPitch > 0.3f && rcRoll > 0.3f)
    //     {
    //         flipingState = FLIPING_FORWARD_RIGHT;
    //     }
    //     else if (rcPitch > 0.3f && rcRoll < -0.3f)
    //     {
    //         flipingState = FLIPING_FORWARD_LEFT;
    //     }
    //     else if (rcPitch < -0.3f && rcRoll > 0.3f)
    //     {
    //         flipingState = FLIPING_BACKWARD_RIGHT;
    //     }
    //     else if (rcPitch < -0.3f && rcRoll < -0.3f)
    //     {
    //         flipingState = FLIPING_BACKWARD_LEFT;
    //     }
    // }
    // }
    if (flipingState != FLIPING_GET_DIR)
    {
        rcRollCnt = 0;
        rcPitchCnt = 0;
    }
    // rt_kprintf("flipingState:%d\t", flipingState);
    // rt_kprintf("rcRollCnt: %d\t", rcRollCnt);
    // rt_kprintf("rcPitchCnt: %d\n", rcPitchCnt);
    return flipingState;
}
void fc_flip_ctrl(int direction)
{

    if (flipingState == FLIPING_IDLE)
    {
        if (direction == 1)
        {
            flipingDir = FLIPING_FORWARD;
        }
        else if (direction == 2)
        {
            flipingDir = FLIPING_BACKWARD;
        }
        else if (direction == 3)
        {
            flipingDir = FLIPING_LEFT;
        }
        else if (direction == 4)
        {
            flipingDir = FLIPING_RIGHT;
        }
        flipingState = FLIPING_SET;
    }
}
void perform_fliping(setPoint_t *set, float anglePitch, float angleRoll)
{
#define MAX_ANGLE 270
#define STOP_ANGLE 270
#define FLIP_GAIN_P 6
#define FLIP_GAIN_R 6
    float setPitchRate = 0;
    float setRollRate = 0;
    switch (flipingDir)
    {
    case FLIPING_FORWARD:
        if (anglePitch < STOP_ANGLE)
        {
            setPitchRate = (MAX_ANGLE - anglePitch) * FLIP_GAIN_P;
        }
        else
        {
            setPitchRate = 0;
            // FC_ERROR_RESET(FC_FLIPING_MODE);
            flipingState = FLIPING_END;
        }
        break;
    case FLIPING_BACKWARD:
        if (anglePitch > -STOP_ANGLE)
        {
            setPitchRate = (-MAX_ANGLE - anglePitch) * FLIP_GAIN_P;
        }
        else
        {
            setPitchRate = 0;
            // FC_ERROR_RESET(FC_FLIPING_MODE);
            flipingState = FLIPING_END;
        }
        break;
    case FLIPING_LEFT:
        if (angleRoll > -STOP_ANGLE)
        {
            setRollRate = (-MAX_ANGLE - angleRoll) * FLIP_GAIN_R;
        }
        else
        {
            setRollRate = 0;
            // FC_ERROR_RESET(FC_FLIPING_MODE);
            flipingState = FLIPING_END;
        }
        break;
    case FLIPING_RIGHT:
        if (angleRoll < STOP_ANGLE)
        {
            setRollRate = (MAX_ANGLE - angleRoll) * FLIP_GAIN_R;
        }
        else
        {
            setRollRate = 0;
            // FC_ERROR_RESET(FC_FLIPING_MODE);
            flipingState = FLIPING_END;
        }
        pro_event_send(PRO_EVENT_FLIP_DONE);
        break;
    default:
        break;
    }
    set->AngleRate.pitch = setPitchRate;
    set->AngleRate.roll = setRollRate;
    // rt_kprintf("setPitchRate:%d/%d\t setRollRate:%d/%d\n",(int)setPitchRate, (int)anglePitch, (int)setRollRate,(int)angleRoll);
}

void flitingHandle(setPoint_t *set, usercmd_t *usercmd, zdrone_state_t *state)
{

    static uint32_t msPrev = 0;
    // static uint32_t flipStartTime = 0;
    static float speedUpMax = 0;
    static float tempThrust = 0;
    static float flipBeginThrust = 0;
    static float flipTimeout = 0;
    static float angleRoll = 0;
    static float anglePitch = 0;
    static float TempPz = 0;
    // static int16_t setRollRate = 0;
    // static int16_t setPitchRate = 0;
    // const int16_t flipRate = 20;

    float dt = (float)(millis() - msPrev) * 0.001f;
    msPrev = millis();

    angleRoll += (state->attitudeRate.roll * dt);
    anglePitch += (state->attitudeRate.pitch * dt);

    switch (flipingState)
    {
    case FLIPING_IDLE:
        fc_enbale_fliting(usercmd, dt);
        // rt_kprintf("FLIPING_IDLE\n");
        break;
    case FLIPING_GET_DIR:
        getFlipingDir(usercmd);
        // rt_kprintf("FLIPING_GET_DIR\n");
        break;
    case FLIPING_SET:
        // flipStartTime = millis();
        flipBeginThrust = fc_control.thrust;
        tempThrust = fc_control.thrust;
        speedUpMax = 80;
        angleRoll = 0;
        anglePitch = 0;
        TempPz = pidData[PID_POSITION_VZ].kp;
        FC_ERROR_SET(FC_FLIPING_MODE);
        flipingState = FLIPING_SPEEDUP;
        // rt_kprintf("FLIPING_SET\n");
        break;
    case FLIPING_SPEEDUP:
        if (state->vel.z < speedUpMax)
        {
            fc_control.thrust += 20;
            flipTimeout += dt;
            if (flipTimeout > 0.5f)
            {
                flipTimeout = 0;
                flipingState = FLIPING_SLOWDOWN;
            }
        }
        else
        {
            flipingState = FLIPING_SLOWDOWN;
        }
        // rt_kprintf("FLIPING_SPEEDUP:%d\n", (int)fc_control.thrust);
        break;
    case FLIPING_SLOWDOWN:
        if (fc_control.thrust > tempThrust)
        {
            fc_control.thrust -= 20;
        }
        else
        {
            flipTimeout = 0;
            flipingState = FLIPING_FLIPING;
        }
        // rt_kprintf("FLIPING_SLOWDOWN:%d\n", (int)fc_control.thrust);
        break;
    case FLIPING_FLIPING:
        flipTimeout += dt;
        if (flipTimeout > 0.5)
        {
            flipTimeout = 0;
            flipingState = FLIPING_END;
            // FC_ERROR_RESET(FC_FLIPING_MODE);
        }
        // rt_kprintf("flipTimeout:%d ", (int)(flipTimeout * 1000));
        perform_fliping(set, anglePitch, angleRoll);
        if (flipingState != FLIPING_FLIPING)
        {
            // fc_control.thrust = flipBeginThrust+50;
            flipTimeout = 0;
        }
        // rt_kprintf("FLIPING_FLIPING:%d, %d\n",(int)anglePitch, (int)angleRoll);
        break;
    case FLIPING_END:
        fc_control.thrust = flipBeginThrust;
        pidReset(&pidData[PID_ANGLE_PITCH]);
        pidReset(&pidData[PID_ANGLE_ROLL]);
        // pidReset(&pidData[PID_POSITION_VZ]);
        // pidReset(&pidData[PID_POSITION_Z]);
        pidReset(&pidData[PID_POSITION_X]);
        pidReset(&pidData[PID_POSITION_Y]);
        pidReset(&pidData[PID_POSITION_VX]);
        pidReset(&pidData[PID_POSITION_VY]);
        FC_ERROR_RESET(FC_FLIPING_MODE);
        flipingState = FLIPING_RECOVER;
        flipTimeout = 0;
        pidData[PID_POSITION_VZ].kp = TempPz * 1.5f;
        break;
    case FLIPING_RECOVER:
        if (flipTimeout++ < 250)
        {
            set->Pos.x = state->pos.x;
            set->Pos.y = state->pos.y;
            // set->Pos.z = state->pos.z;

            pidReset(&pidData[PID_POSITION_X]);
            pidReset(&pidData[PID_POSITION_Y]);
            // pidReset(&pidData[PID_POSITION_VX]);
            // pidReset(&pidData[PID_POSITION_VY]);
        }
        else
        {
            pidData[PID_POSITION_VZ].kp = TempPz;
            flipingState = FLIPING_IDLE;
            flipTimeout = 0;
        }
        pro_event_send(PRO_EVENT_FLIP_DONE);
        break;
    default:
        break;
    }
}
void flipinghandle_fun(void)
{
    flitingHandle(&setPoint, &usercmd, &zdrone_state);
}
void heading_mode_handle(setPoint_t *set, usercmd_t *usercmd, zdrone_state_t *state)
{
    // NOTE - 右摇杆左右控制ROLL
    static bool rolling = false;
    static bool pitching = false;
    const uint8_t rcRollPichMax = pidConfig()->rcRollPitchMax;
    if (ABS(usercmd->rcChannel[RC_RLR]) > RC_DEAD_BAND)
    {
        set->Pos.y = state->pos.y;
        set->Vel.y = 0;
        set->Angle.roll = constrainf((usercmd->rcChannel[RC_RLR] / (RC_MID - RC_DEAD_BAND)) * rcRollPichMax, -rcRollPichMax, rcRollPichMax);
        rolling = true;
        // rt_kprintf("set->Angle.roll: %d\n",(int)(set->Angle.roll*100));
    }
    else
    {
        if (rolling)
        {
            set->Pos.y = state->pos.y;
            if (ABS(state->vel.y) < 0.1f) // until cross 0 point
            {
                rolling = false;
            }
        }
    }
    // NOTE - 右摇杆上下控制PITCH
    if (ABS(usercmd->rcChannel[RC_RUD]) > RC_DEAD_BAND)
    {
        set->Pos.x = state->pos.x;
        set->Vel.x = 0;
        set->Angle.pitch = constrainf((usercmd->rcChannel[RC_RUD] / (RC_MID - RC_DEAD_BAND)) * rcRollPichMax, -rcRollPichMax, rcRollPichMax);
        pitching = true;
    }
    else
    {
        if (pitching)
        {
            set->Pos.x = state->pos.x;
            if (ABS(state->vel.x) < 0.1f) // until cross 0 point
            {
                pitching = false;
            }
        }
    }
}

void setpoint_handle(setPoint_t *set, usercmd_t *usercmd, zdrone_state_t *state)
{

    // set->manual[X] = 0;
    // set->manual[Y] = 0;

    if ((get_vbat_voltage_mv() < 6500 && FC_ERROR_CHECK(FC_ARMED)) ||
        (get_vbat_voltage_mv() < 6600 && !FC_ERROR_CHECK(FC_ARMED)))
    {
        FC_ERROR_SET(FC_LOW_POWER);
        lowRcCnt = 100;
        if (FC_ERROR_CHECK(FC_AIRBORN) && !FC_ERROR_CHECK(FC_FLIPING_MODE))
        {
            FC_ERROR_SET(FC_LANDING);
        }
    }
    else
    {
        if (lowRcCnt > 0)
        {
            lowRcCnt--;
        }
        else
        {
            FC_ERROR_RESET(FC_LOW_POWER);
        }
    }

    rc_data_fuc_handle(usercmd);
    headless_mode_switch(usercmd);

    if (FC_ERROR_CHECK(FC_ARMED))
    {

        if (flipingState == FLIPING_IDLE)
        {
            if (FC_ERROR_CHECK(FC_HEADLESS_MODE))
                headless_mode_handle(set, usercmd, state);
            else
            {
                usercmd->headFreeCh[RC_RUD] = 0;
                usercmd->headFreeCh[RC_RLR] = 0;
            }

            // manual moving
            if (ABS(usercmd->rcDeflection[RC_RUD]) > 0.01f || ABS(usercmd->headFreeCh[RC_RUD]) > 0.01f || ABS(usercmd->rcDeflection[RC_RLR]) > 0.01f || ABS(usercmd->headFreeCh[RC_RLR]) > 0.01f)
            {
                if (ABS(usercmd->headFreeCh[RC_RLR]) > 0.01f)
                {
                    movingStoptime_y = millis();
                    set->manual[Y] = 1;
                    set->Pos.y = state->pos.y;
                    set->Angle.roll = usercmd->headFreeCh[RC_RLR] * usercmd->maxRollAndPicthAngle;
                }
                else
                {
                    if (ABS(usercmd->rcDeflection[RC_RLR]) > 0.01f)
                    {
                        movingStoptime_y = millis();
                        set->manual[Y] = 1;
                        set->Pos.y = state->pos.y;
                        set->Angle.roll = usercmd->rcDeflection[RC_RLR] * usercmd->maxRollAndPicthAngle;
                    }
                }

                if (ABS(usercmd->headFreeCh[RC_RUD]) > 0.01f)
                {
                    movingStoptime_x = millis();
                    set->manual[X] = 1;
                    set->Pos.x = state->pos.x;
                    set->Angle.pitch = usercmd->headFreeCh[RC_RUD] * usercmd->maxRollAndPicthAngle;
                }
                else
                {
                    if (ABS(usercmd->rcDeflection[RC_RUD]) > 0.01f)
                    {
                        movingStoptime_x = millis();
                        set->Pos.x = state->pos.x;
                        set->manual[X] = 1;
                        set->Angle.pitch = usercmd->rcDeflection[RC_RUD] * usercmd->maxRollAndPicthAngle;
                    }
                }
            }
            else
            {
                if (movingStoptime_x != 0)
                {

                    vx_kp_boost = 2.f;
                    set->manual[X] = 0;
                    set->Pos.x = state->pos.x;
                    if (millis() > (movingStoptime_x + 100) && (ABS(state->vel.x) < 0.1F)) // delay some time
                    {
                        movingStoptime_x = 0;
                        vx_kp_boost = 1.0f;
                    }
                }
                if (movingStoptime_y != 0)
                {
                    vy_kp_boost = 2.f;
                    set->manual[Y] = 0;
                    set->Pos.y = state->pos.y;
                    if (millis() > (movingStoptime_y + 100) && (ABS(state->vel.y) < 0.1F)) // delay some time
                    {
                        movingStoptime_y = 0;
                        vy_kp_boost = 1.0f;
                    }
                }
            }
        }

        // NOTE - 左摇杆左右控制YAW角速度
        if (ABS(usercmd->rcDeflection[RC_LLR]) > 0.01f)
        {
            set->AngleRate.yaw = -usercmd->rcDeflection[RC_LLR] * usercmd->maxYawRate;
            set->Angle.yaw = state->attitude.yaw;
        }
        if (FC_ERROR_CHECK(FC_RC_LOST)) // 持续下降
        {
            if (FC_ERROR_CHECK(FC_AIRBORN))
                FC_ERROR_SET(FC_LANDING);
        }
        // NOTE -左摇杆上下控制油门
        if (ABS(usercmd->rcDeflection[RC_LUD]) > 0.01f)
        {
            // float setVelZ = constrainf((usercmd->rcChannel[RC_LUD] / (RC_MID - RC_DEAD_BAND)) * VEL_Z_LIMIT, -VEL_Z_LIMIT, VEL_Z_LIMIT);
            float setVelZ = usercmd->rcDeflection[RC_LUD] * (float)usercmd->maxVel_Z;
            if (setVelZ < 0)
            {
                set->Vel.z = setVelZ;
            }
            else
            {
                if (isReachHeightLimit == false)
                {
                    set->Vel.z = setVelZ;
                }
                else
                {
                    set->Vel.z = 0;
                }
            }
            set->manual[Z] = 1;
            set->Pos.z = state->pos.z;
            movingStoptime_z = millis();
        }

        if (movingStoptime_z != 0)
        {
            set->Pos.z = state->pos.z;
            if (millis() > (movingStoptime_z + 100)) // delay some time
            {
                movingStoptime_z = 0;
                set->manual[Z] = 0;
            }
        }

        // rt_printf_float("maxYawRate: ", usercmd->maxRollAndPicthAngle, " \t");
        // rt_printf_float("maxRollAndPicthAngle: ", usercmd->maxRollAndPicthAngle, " \t");
        // rt_printf_float("maxVel_Z: ", usercmd->maxVel_Z, " \t");
        // rt_printf_float("setVelZ: ", set->Vel.z, " \t");
        // rt_printf_float("RC_LUD: ", usercmd->rcDeflection[RC_LUD], " \n");

        if (FC_ERROR_CHECK(FC_MANUAL_MODE))
        {
            fc_control.thrust = constrainf((usercmd->rcChannel[RC_LUD] / (RC_MID - RC_DEAD_BAND)) * 1000 + 500, 0, 1000);
            // rt_kprintf("fc_control.thrust: %d\n", (int)fc_control.thrust);
        }

        // NOTE flying height limit
        float current_height = state->pos.z - groundHeight;
        if (current_height >= FLY_HEIGHT_LIMIT)
        {
            isReachHeightLimit = true;
        }
        else
        {
            isReachHeightLimit = false;
        }
    }
    else
    {
        headingStart = 0;
    }

    // land_by_rc(usercmd);

    // if (pro_autotakingoff)
    // {
    //     if (takeofftime == 0 && (millis() > (landtime + 2000)))
    //     {
    //         // NOTE AUTOTAKINGOFF

    //         AutoTakingOffHandle(set, state);
    //         takeofftime = millis();
    //         LOG_D("takeofftime:%d\n", takeofftime);
    //     }
    // }
}

// void TakeOffHandle(setPoint_t *set, zdrone_state_t *state)
// {
// }
extern void ano_tc_report(void);
extern void rgb_switch(bool triger);
void UserCmdHandleTask()
{
    while (1)
    {

        if (get_fc_main_state() == true)
            return;

        setpoint_handle(&setPoint, &usercmd, &zdrone_state);
        rgb_switch(usercmd.rcChannel[RC_KEY_VALUE] & (1 << BALL_ADJ_LLEFT));

        rt_thread_mdelay(THREAD_TIME_MS);
    }
}

int user_cmd_task_entry()
{
    rt_thread_t usercmd_thread = rt_thread_create("usercmd", UserCmdHandleTask, RT_NULL, 1024, 10, 10);
    if (pidConfig()->saveFlag != PID_CONFIG_SAVE_FLAG)
    {
        pidConfig()->rcRollPitchMax = ROLL_PITCH_ANGLE_LIMIT;
    }
    if (usercmd_thread != RT_NULL)
    {
        rt_thread_startup(usercmd_thread);
    }
    else
    {
        LOG_E("usercmd_thread create failed.");
    }
    return 0;
}
int usbcmd(int argc, char *argv[])
{
    if (rt_strcmp(argv[1], "arm") == 0)
    {
        FC_ERROR_SET(FC_ARMED);
        rt_kprintf("usbcmd: arm\r\n");
    }
    if (rt_strcmp(argv[1], "disarm") == 0)
    {
        FC_ERROR_RESET(FC_ARMED);
        rt_kprintf("usbcmd: arm\r\n");
    }
    return 0;
}
// MSH_CMD_EXPORT(usbcmd, usb cmd);
ZINO_APP_EXPORT(user_cmd_task_entry);
